#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  sonar,          sensorSONAR_cm)
#pragma config(Sensor, dgtl7,  rightLimit,     sensorTouch)
#pragma config(Sensor, dgtl8,  leftLimit,      sensorTouch)
#pragma config(Sensor, dgtl9,  bumper,         sensorTouch)
#pragma config(Motor,  port2,           RM,            tmotorVex269, openLoop)
#pragma config(Motor,  port3,           LM,            tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port4,           servo,         tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int rmSpeed=30;
int lmSpeed=42;
void forward(int revs){
  int turns=revs*360;
  //SensorValue[rightEncoder]=0;
  //SensorValue[leftEncoder]=0;
  while(SensorValue[rightEncoder]<turns){
    if(SensorValue[rightEncoder]>-SensorValue[leftEncoder]){
      motor[RM]=rmSpeed;
      motor[LM]=lmSpeed;
    }
    else if(-SensorValue[leftEncoder]>SensorValue[rightEncoder]){
      motor[RM]=rmSpeed;
      motor[LM]=lmSpeed;
    }
      else{
      motor[RM]=rmSpeed;
      motor[LM]=lmSpeed;
  }
 }
}

void turnLeft(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
    while(SensorValue[rightEncoder]<180){
      motor[RM]=127;
      motor[LM]=-127;
    }
   }

void turnRight(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
    while(SensorValue[rightEncoder]>-180){
      motor[RM]=-127;
      motor[LM]=127;
    }
   }

void slightRight(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
    while(SensorValue[rightEncoder]>-100){
      motor[RM]=-127;
      motor[LM]=127;
    }
   }
void slightLeft(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
    while(SensorValue[rightEncoder]<100){
      motor[RM]=127;
      motor[LM]=-127;
    }
   }
void backwards(int revs){
  int turns=revs*360;
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  while(SensorValue[rightEncoder]>-90){
    if(SensorValue[rightEncoder]>-SensorValue[leftEncoder]){
      motor[RM]=-50;
      motor[LM]=-57;
    }
    else if(-SensorValue[leftEncoder]>SensorValue[rightEncoder]){
      motor[RM]=-57;
      motor[LM]=-50;
    }
      else{
      motor[RM]=-50;
      motor[LM]=-50;
  }
 }
}
void pause(int wait){
  motor[RM]=0;
  motor[LM]=0;
  wait1Msec(wait);
}
void checkDirection(){
  motor[RM]=0;
  motor[LM]=0;
  backwards(1);
  motor[RM]=0;
  motor[LM]=0;
  motor[servo]=-50;
  wait1Msec(750);
  motor[servo]=127;
	wait1Msec(750);
   int lCheck=SensorValue[sonar];
  motor[servo]=-127;
  wait1Msec(750);
   int rCheck=SensorValue[sonar];
  motor[servo]=-50;
  wait1Msec(100);
      if(lCheck>rCheck){
        turnLeft();
    }
    else if(lCheck<rCheck){
        turnRight();
    }
    else{
        turnRight();
    }

 }

 void halt(){
  if(SensorValue(bumper)==1){
         motor[RM]=0;
         motor[LM]=0;
         wait1Msec(10000);
  }
}

task main(){
  SensorValue[rightEncoder]=0;
  SensorValue[leftEncoder]=0;
  //if(SensorValue(bumper)==1){
  //while(motor(RM)==0  && motor(LM)==0){
  while(1==1){
    halt();
    while(SensorValue[sonar]>10 || SensorValue[sonar]<1){
    if(SensorValue[rightLimit]==0 && SensorValue[leftLimit]==0){
          forward(1);
        }
    else if(SensorValue[leftLimit]==1){
    backwards(1);
    pause(250);
    slightRight();
    }
      else if(SensorValue[rightLimit]==1){
      backwards(2);
      pause(250);
      slightLeft();
      }
      halt();
    }
    checkDirection();
 }
}
//}
//}
